#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from manipulation.msg import TargetsList
from threading import Thread
import time

class DebugTargetsList(Node):
    def __init__(self):
        super().__init__('debug_targets_list')
        self.pub = self.create_publisher(TargetsList, 'detection_targets', 1)

        self.rate = self.create_rate(10)  # 10 Hz
        self.grasped = [False, False, False]

        self.thread = Thread(target=self.publish_targets)
        self.thread.start()
        self.update_thread = Thread(target=self.update_grasped)
        self.update_thread.start()

        self.get_logger().info("DebugTargetsList node has been started.")
    
    def update_grasped(self):
        time.sleep(1)
        self.get_logger().info(f"Grasped status: {self.grasped}")
        while True:
            try:
                idx = int(input("Change grasped status (press Enter to toggle): "))
                self.grasped[idx] = not self.grasped[idx]
                self.get_logger().info(f"Grasped status updated: {self.grasped}")
            except:
                self.get_logger().error(f"Invalid input. Please enter a valid integer index in {list(range(len(self.grasped)))}.")

    def publish_targets(self):
        while rclpy.ok():
            msg = TargetsList()
            msg.names = ['1', '2', '3']
            msg.grasped = self.grasped
            self.pub.publish(msg)
            self.rate.sleep()

if __name__ == '__main__':
    rclpy.init()
    rclpy.spin(DebugTargetsList())
    rclpy.shutdown()
